//
// Created by sxy on 2020/12/8.
//

#include "publisher/path_publisher.h"

namespace lidar_localization{
PathPublisher::PathPublisher(ros::NodeHandle &nh, std::string topic_name,
                             std::string frame_id, int buff):nh_(nh) {
    publisher_=nh_.advertise<nav_msgs::Path>(topic_name,buff);
    frame_id_=frame_id;
    path.header.frame_id=frame_id_;
}

void PathPublisher::PublishData(const Eigen::Matrix4f &transform_matrix, ros::Time time) {
    path.header.stamp=time;
    geometry_msgs::PoseStamped this_pose_stamp;
    this_pose_stamp.header.stamp=time;
    this_pose_stamp.pose.position.x = transform_matrix(0,3);
    this_pose_stamp.pose.position.y = transform_matrix(1,3);
    this_pose_stamp.pose.position.z = transform_matrix(2,3);
    Eigen::Quaternionf Qwb;
    Qwb = transform_matrix.block<3,3>(0,0);
    this_pose_stamp.pose.orientation.x = Qwb.x();
    this_pose_stamp.pose.orientation.y = Qwb.y();
    this_pose_stamp.pose.orientation.z = Qwb.z();
    this_pose_stamp.pose.orientation.w = Qwb.w();
    this_pose_stamp.header.frame_id=frame_id_;
    path.poses.push_back(this_pose_stamp);
    publisher_.publish(path);
}

void PathPublisher::Publish(const Eigen::Matrix4f &transform_matrix, double time) {
    ros::Time ros_time((float)time);
    PublishData(transform_matrix, ros_time);
}

}